# coding: utf-8

from numpy import array, deg2rad, sqrt
from math import sin, cos
from robotics.io.pcd import load_point_cloud
from robotics.registration.util import transform_point_cloud
from robotics.registration.correspondence.biway import find_correspondences


def rotation_around_y_estimation(deg):
    r = deg2rad(deg)
    return array([
        [cos(r), 0, -sin(r)],
        [0, 1, 0],
        [sin(r), 0, cos(r)]
    ])


p1 = load_point_cloud(
    '/Users/yfwz100/Documents/notebook/icp/robot/data/office2/1-[0.00,0.00,0.00]-[0.00,0.00,0.00].pcd') * 10
p2 = load_point_cloud(
    '/Users/yfwz100/Documents/notebook/icp/robot/data/office2/2-[0.07,0.00,-0.19]-[0.00,-0.52,0.00].pcd') * 10


def different(deg, translate):
    global p1, p2

    print("Configuration: %d, (%.2f, %.2f, %.2f)" % (deg, translate[0], translate[1], translate[2]))

    p1t = transform_point_cloud(p1, rotation_around_y_estimation(deg), translate)
    p1c, p2c = find_correspondences(p1t, p2)
    diff = abs(p1c - p2c) ** 2

    s = diff.sum(axis=1)
    print("Shape: %s" % str(s.shape))
    print("Mean Error: %f" % sqrt(s).mean())


different(-30, [1.9, 0, 0.7])
different(-28.28, [1.38, 0, 0.4])
different(-28.54, [2.29, 0, -0.09])
different(-29.28, [1.38, 0, 0.3])
